#!/usr/bin/env python

""" Tool to program the AS5162 magnetic rotary sensor over UART

This software is by Berwyn Hoyt
If you use this and feel kind, please let my curiosity know at:
    http://brush.co.nz/contact
It is distributed under the nicely permissive MIT license specified here:
    http://opensource.org/licenses/mit-license.php

Using this for production programming is much easier than the Austria
Microsystems tools which require downloading 1GB of Labview runtimes.
Since this tool is only about 10k big, I am quite proud to discover that
this represents a tool compression ratio of approximately 99.999%
Of course, that assumes that you already have python installed (if you
don't then it's only a 16MB download -- still a 67% tool compression)
This tool was designed and tested to produce the same results as the AS GUI

This has been tested using python 2.7 under Windows but should also work
just fine under Linux.

Notes:
    The datasheet is rather sketchy about what the various parameters mean.
    It would have been nice to have the following info from Austria Microsystems,
    but I have discovered it by trial and error from their GUI tool:

        Vout = Vdd x cordic x gain / 0x2000000 + Vdd x 2 x offset / gain

    where:
     - gain, signed 17 bit (Setting it to 2048 makes a 5V ramp over 360 deg)
     - break point (BP), unsigned 14-bit: the cordic value where a high output changes to low (unsigned 14-bit)
     - offset, signed 20-bit

    The AMS AS5x6y GUI tool is still useful for visualizing these parameters.  You can use
    its visualizations component without downloading the whole 700MB NI VISA extension
    but you can only program the chip with the VISA extensions.

    The workings of the linearization points (X1LIN, X2LIN, Y1LIN, Y2LIN) are
    still a mystery and are not clarified in the datasheet.  The GUI tool left
    them as zero, so I do too.

For the record, below are the comms of the AS GUI tool when the "Fuse" button is clicked.
> 55 13 01 00   # write 0x0001 into undocumented register 0x13
< 55 13 01 00   # echo
> 55 00 80 18 55 02 08 00 55 04 02 5e 55 06 ff e4 55 08 9e 11 55 0a 60 00 55 0c 00 00   # write OTP RAM
< 55 00 80 18 55 02 08 00 55 04 02 5e 55 06 ff e4 55 08 9e 11 55 0a 60 00 55 0c 00 00   # echo
> 55 80         # read each OTP word
< 55 80 80 18   # echo & response ...
> 55 82
< 55 82 08 00
> 55 84 
< 55 84 02 5e 
> 55 86 
< 55 86 ff e4 
> 55 88 
< 55 88 9e 11 
> 55 8a 
< 55 8a 60 00
> 55 8c 
< 55 8c 00 00 
> 55 41 51 62   # fuse command
< 55 41 51 62   # echo
> 55 80         # read each OTP word
< 55 80 80 18   # echo & respond
> 55 82 
< 55 82 08 00 
> 55 84 
< 55 84 02 5e 
> 55 86 
< 55 86 ff e4 
> 55 88 
< 55 88 9e 11 
> 55 8a 
< 55 8a 60 00 
> 55 8c 
< 55 8c 00 00 
> 55 22 60 00   # write CTRL.R1K10K=3
< 55 22 60 00   # echo
> 55 22 60 00   # same again
< 55 22 60 00
> 55 80         # read each OTP word
< 55 80 80 18   # echo & response ...
> 55 82 
< 55 82 08 00 
> 55 84 
< 55 84 02 5e 
> 55 86 
< 55 86 ff e4 
> 55 88 
< 55 88 9e 11 
> 55 8a 
< 55 8a 60 00 
> 55 8c 
< 55 8c 00 00 
> 55 22 40 00   # write CTRL.R1K10K=2
< 55 22 40 00   # echo
> 55 22 40 00   # same again
< 55 22 40 00 
> 55 80         # read each OTP word
< 55 80 80 18   # echo and response ...
> 55 82 
< 55 82 08 00 
> 55 84 
< 55 84 02 5e 
> 55 86 
< 55 86 ff e4 
> 55 88 
< 55 88 9e 11 
> 55 8a 
< 55 8a 60 00 
> 55 8c 
< 55 8c 00 00 
> 55 8e         # read AMS field of OTP (even though it didn't program that field)
< 55 8e 25 04 
> 55 60 51 62   # pass2func command
< 55 60 51 62   # echo
"""

import sys, time, platform
try:
    import serial
except ImportError:
    print >>sys.stderr, "You must install pyserial from: http://pyserial.sourceforge.net"
    sys.exit(2)

from storage import storage

class Error(Exception): pass
class TimeoutError(Error): pass
class ConnectError(Error): pass
class SetupError(Error): pass
class MagnetError(Error): pass
class FuseError(Error): pass

def to_hex(string):
    """ Return hex version of string """
    return ' '.join('%02x' % ord(c) for c in string)

# read-only registers (addr, mask)
STATUS = (0x28, 0xff)
CORDICOUT = (0x29, 0x3fff)
DSPOUT = (0x2b, 0xffff)
AGCVALUE = (0x2d, 0xff)
MAG = (0x2e, 0xff)

# Writable registers
BAUDREG = 0x20
CTRL = 0x22
FUSE = 0x41
PASS2FUNC = 0x60

# Note: access the OTP section using the get_otp and set_otp functions, not directly via register access

class Device(serial.Serial):
    """ Implement AS5162 or similar UART chip protocol.
        - use get() or set() to set chip registers
        - to set otp bits, use get_otp() and then set self.otp fields, and then set_otp() to store """

    otp_layout = [  ('ams', 18), ('custid', 7), ('x2lin', 4), ('x1lin', 4), ('y1lin', 3), ('y2lin', 3),
                    ('clh', 9), ('cll', 9), ('offset', 20), ('gain', 17), ('bp', 14), ('anglerng', 1),
                    ('diag_high', 1), ('quaden', 2), ('airgapsel', 1), ('hystsel', 2), ('filtercfg', 2),
                    ('unused', 3), ('red_add', 4), ('red_bit', 3), ('cust_lock', 1)]

    def __init__(self, port, vdd=5, baudrate=9600, timeout=1, debug=0, connect=True, **kwargs):
        """ Init AS5162 device instance
            debug defines volume of debugging output to print (0 is none, 1=messages)
            Highest standard baudrate that works seems to be 57600
            Pass other args to serial.Serial() """
        #pylint: disable=super-init-not-called
        self.debug = debug
        self.otp = storage()
        self.otp_original = self.otp_new = None     # for the use of self.verify
        self.serial_port = port
        self.vdd = vdd
        kwargs.update(dict(baudrate=baudrate, timeout=timeout, parity=serial.PARITY_EVEN, stopbits=serial.STOPBITS_ONE))
        self.open_args = kwargs
        if connect:
            self.connect()

    def connect(self):
        """ Connect using args given in constructor """
        serial.Serial.__init__(self, self.serial_port, **self.open_args)
        try:
            self.get(STATUS)
        except TimeoutError:
            raise ConnectError("Could not contact device on %s; device may be disconnected or already programmed" % self.serial_port)

    if platform.system() == 'Windows':
        # Windows serial doesn't implement this like linux does, and it should.
        def flush(self):
            """ Wait for all characters to be sent """
            while self.outWaiting(): pass

    def debug_out(self, string):
        """ Print string if self.debug is True """
        if self.debug >= 2:
            sys.stdout.write(string)

    def set(self, addr, data):
        """ Write 16-bit data to addr """
        string = '\x55%c%c%c' % (addr, data&0xff, data>>8)
        self.debug_out('> %s\n' % to_hex(string))
        self.write(string)
        chars = self.read(len(string))
        if len(chars) < len(string):
            raise TimeoutError("Echo only received %d chars (%s) after sending write command (%s)" % (len(chars), to_hex(chars), to_hex(string)))

    def _get(self, addr):
        """ Read 2 chars from addr """
        self.flushInput()           # clear any left-over characters from a previous communication attempt
        string = '\x55%c' % (addr|0x80)
        self.debug_out('> %s < ' % to_hex(string))
        self.write(string)
        chars = self.read(len(string) + 2)
        if len(chars) < len(string) + 2:
            raise TimeoutError("Only received %d chars (%s) after sending read command (%s)" % (len(chars), to_hex(chars), to_hex(string)))
        chars = chars[len(string):]
        self.debug_out('%s\n' % to_hex(chars))
        return chars

    def get(self, addr):
        """ Read 16-bits from addr
            addr may be an integer or an (addr, mask) tuple """
        mask = 0xffff
        if type(addr) == tuple:
            addr, mask = addr
        self.flushInput()           # clear any left-over characters from a previous communication attempt
        chars = self._get(addr)
        value = ord(chars[0]) | ord(chars[1])<<8
        return value & mask

    def get_otp(self):
        """ Read the OTP RAM image from the chip into self.otp storage dict """
        otp = ''.join(self._get(i) for i in xrange(0, 16, 2))
        otp = otp[::-1] # reverse the string by extended slice
        otp = sum(ord(otp[i])<<i*8 for i in xrange(16))   # turn into a 128-bit number
        for name, bits in self.otp_layout:
            mask = ((1<<bits)-1)
            value = int(otp & mask)
            # handle list of signed values properly -- sign extend to int
            if name in ['offset', 'gain']:
                if value & (1<<(bits-1)):
                    value = (-1&~mask) | value
            self.otp[name] = value
            otp >>= bits

    def set_otp(self):
        """ Write the self.otp storage dict into the OTP RAM image on chip """
        otp = 0
        # turn into 128-bit number
        for name, bits in reversed(self.otp_layout):
            otp <<= bits
            otp |= int(self.otp.get(name, 0)) & ((1<<bits)-1)
        otp = [otp>>(i*8)&0xff for i in xrange(16)]   # turn into list of bytes
        if self.debug >= 1:
            print "Programming OTP with %s (= %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x, MSB first)" % ((self.str_otp(self.otp),)+tuple(otp))
        otp.reverse()
        for i in xrange(0, 16, 2):
            self.set(i, otp[i] | otp[i+1]<<8)

    def str_otp(self, otp):
        """ Translate otp contents into a sorted printable string """
        return ', '.join( "%s=%#x" % (name, otp[name]) for name in sorted(otp.keys()) )

    def filter_otp(self, source, ref):
        """ Return a copy of source otp dict containing only the keys in ref dict """
        filtered = source.copy()
        for key in source.keys():
            if key not in ref:
                filtered.pop(key)
        return filtered

    def diff_otp(self, written, read):
        """ Return comparison of written and read otp dicts """
        return self.str_otp(written), self.str_otp(self.filter_otp(read, written))

    def verify_otp(self):
        """ Read otp and return whether OTP matches self.otp 
            Caller may access the two verified storages for errors messages as self.otp_original and self.otp_new """
        otp_save = storage(self.otp.copy())
        self.otp_original = storage(self.otp.copy())
        self.get_otp()
        self.otp_new = storage(self.otp.copy())
        # compare original with new values (only compare values that exist in original)
        result = True
        for name, value in self.otp_original.iteritems():
            # don't verify factory settings: we don't set them, and they vary
            if name == 'ams':
                continue
            if self.otp_new[name] != value:
                result = False
        # restore otp to what it was (a verify function shouldn't have side-effects)
        self.otp.update(otp_save)
        return result

    def get_magnet(self):
        """ Read and return current (CORDIC, AGC) of the sensor """
        # Unsure whether need to set CTRL twice like in fuse(), so do it anyway
        self.set(CTRL, 0x0080)    # unreset DSP
        self.flush()
        time.sleep(0.005)             # the AS GUI tool waits 1.5s. Not sure it's necessary
        self.set(CTRL, 0x0080)    # unreset DSP
        time.sleep(1.5)             # the AS GUI tool waits 1.5s. Not sure it's necessary
        cordic = self.get(CORDICOUT)
        agc = self.get(AGCVALUE)
        self.set(CTRL, 0x0000)    # unreset DSP
        self.flush()
        time.sleep(0.005)             # the AS GUI tool waits 1.5s. Not sure it's necessary
        self.set(CTRL, 0x0000)    # unreset DSP
        return cordic, agc

    def get_angle(self, cordic=None):
        """ Return current angle of magnet in degrees based on cordic (with BP=0)
            If cordic is None then read cordic from chip """
        if cordic is None:
            cordic, agc = self.get_magnet()
            if agc >= 255:
                raise MagnetError("The magnet is too far away for accurage sensing")
        return float(cordic*360) / 0x3fff

    def setup(self, angle_range=(0, 359.9999), voltage_range=(0, 5), breakpoint=0, **kwargs):
        """ Set up the OTP RAM on the chip
            Unless overridden in kwargs, the following will be calculated and set from
            angle_range, voltage_range and breakpoint: cll, clh, gain, offset, bp, anglerng
            - angle_range specifies start/end angle between which the voltage will ramp
              start/end are relative to breakpoint. It can be set to self.get_angle()+constant
            - voltage_range specifies lower and upper voltage
            Specify other OTP parameters as arguments by the names given in the datasheet, e.g. cust_lock=1
            Note: defaults will output a 5V ramp over the full angle range """
        otp = self.otp
        # wrap angles within circle so user can specify get_angle()+x and not worry about 360 deg wrapping
        breakpoint %= 360
        angle_range = (angle_range[0]%360, angle_range[1]%360)
        arc = float(angle_range[1] - angle_range[0])%360

        otp.cll = int(round(min(voltage_range)/self.vdd * 0x1ff))
        otp.clh = int(round((self.vdd-max(voltage_range))/self.vdd * 0x1ff))
        gain = 2048 # this gain rises from 0 to 5V across 360 deg
        gain *= float(voltage_range[1] - voltage_range[0]) / self.vdd   # scale gain to voltage_range
        gain /= arc/360                     # scale gain to angle_range
        otp.gain = int(round(gain))
        otp.bp = int(round(0x3fff * float(breakpoint)/360))
        offset = -2 * gain * angle_range[0]/360 + 4096 * voltage_range[0] / self.vdd
        otp.anglerng = int(arc<22.5)
        otp.offset = int(round(offset))
        otp.update(kwargs)
        self.set_otp()      # put our python values into OTP RAM
        time.sleep(0.010)
        if not self.verify_otp():
            raise SetupError("OTP written to device does not match OTP read back:\nwritten: %s\n   read: %s\nTry it again." % self.diff_otp(self.otp_original, self.otp_new))

    def pass2func(self):
        """ Move chip into function (ADC out) mode """
        self.set(PASS2FUNC, 0x6251)    # note: the datasheet seems to have gotten 0x5162 byte swapped.  This way around actually works

    def fuse(self):
        """ Program OTP data into the chip permanently using programming procedure specified in datasheet 9.7
            You should run setup() first.
            Raises SetupError if data previously in self.otp doesn't match what we read out
              (happens if setup() hasn't been called first)
            Raises FuseError on any errors that may have left partial writes """
        # turn on device here if you have turn-on control
        time.sleep(0.010)
        if not self.verify_otp():
            raise SetupError("OTP written to device does not match OTP read back:\nwritten: %s\n   read: %s\nTry it again." % self.diff_otp(self.otp_original, self.otp_new))
        self.set(FUSE, 0x6251)      # note: the datasheet seems to have gotten 0x5162 byte swapped.  This way around actually works
        self.flush()                # ensure chars all sent before delay starts
        time.sleep(0.010)

        # Must set CTRL twice for some reason (it doesn't always work otherwise)
        self.set(CTRL, 0x60)        # R1K10K = 3; prog with 10k ref
        self.flush()                # ensure chars all sent before delay starts
        time.sleep(0.005)
        self.set(CTRL, 0x60)        # R1K10K = 3; prog with 10k ref
        if self.get(CTRL)&0x60 != 0x60:       # Note: AS GUI does not do this readback check specified in datasheet
            raise FuseError("Error testing fuse of OTP into RAM with 10k reference. AS5162 chip may be bad")

        if not self.verify_otp():
            raise FuseError("OTP written to device with 10k reference does not match OTP fused and read back:\nwritten: %s\n   read: %s\nAS5162 may be bad." % self.diff_otp(self.otp_original, self.otp_new))

        # Must set CTRL twice for some reason (it doesn't always work otherwise)
        self.set(CTRL, 0x40)        # R1K10K = 2; prog with 1k ref
        self.flush()                # ensure chars all sent before delay starts
        time.sleep(0.005)
        self.set(CTRL, 0x40)        # R1K10K = 2
        self.flush()                # ensure chars all sent before delay starts
        time.sleep(0.005)
        if self.get(CTRL)&0x60 != 0x40:       # Note: AS GUI does not do this readback check specified in datasheet
            raise FuseError("Error testing fuse of OTP into RAM with 1k reference. AS5162 chip may be bad")
        if not self.verify_otp():
            raise FuseError("OTP written to device with 1k reference does not match OTP fused and read back:\nwritten: %s\n   read: %s\nAS5162 may be bad." % self.diff_otp(self.otp_original, self.otp_new))


help_msg = """
Usage: as5162.py <serial_port> "device.setup(<angle_range>, <voltage_range>, <breakpoint>, ...); [device.fuse();] [device.pass2func()]"
  for example, to set a 90 deg 5V ramp:
as5162.py com2 "device.setup(angle_range=(0, 90), voltage_range=(0, 5), breakpoint=device.get_angle()+90, filtercfg=3, cust_lock=1)"
  where:
  - angles are in degrees, offset from breakpoint
  - voltages are ramp start/stop levels (= T1Y, T2Y = CLL, CLH)
The above sets OTP registers: cll, clh, gain, offset, bp, anglerng. Others may be set as follows:
  - '...' is a list of OTP parameters using the datasheet names (in lower case)
Note: the OTP will not be permanent unless device.fuse() is invoked""".strip()

if __name__ == '__main__':
    if len(sys.argv) < 2 and platform.node() == 'berwyn':     # for developer's test purposes
        serial_port = 'com22'
        device = Device(serial_port, vdd=5, debug=1)
        device.setup(angle_range=(90, 180), voltage_range=(0.165, 3.13), breakpoint=device.get_angle()-45-90,
            diag_high=0, quaden=0, airgapsel=0, hystsel=0, filtercfg=3)
        device.pass2func()
    else:
        if len(sys.argv) < 3:
            print help_msg
            sys.exit(1)
        serial_port = sys.argv[1]
        device = Device(serial_port, vdd=5, debug=1)
        for parameter in sys.argv[2:]:
            if parameter.startswith('"') and parameter.endswith('"'):
                parameter = parameter[1:-1]
            #pylint: disable=exec-statement
            exec parameter
